#include "TriangularObstacle.h"
#include "TriangularSketcher.h"
#include "GeometryHelper.h"
#include <math.h>

TriangularObstacle::TriangularObstacle(void){
	this->setType("TriangularObstacle");
}

TriangularObstacle::TriangularObstacle( Position * p1, Position * p2, Position * p3 ){
	this->setType("TriangularObstacle");
	this->setP1(p1);
	this->setP2(p2);
	this->setP3(p3);
}

void TriangularObstacle::setP1( Position * p1 ){
	this->p1 = p1;
}

void TriangularObstacle::setP2( Position * p2 ){
	this->p2 = p2;
}

void TriangularObstacle::setP3( Position * p3 ){
	this->p3 = p3;
}

Position * TriangularObstacle::getP1( void ){
	return this->p1;
}

Position * TriangularObstacle::getP2( void ){
	return this->p2;
}

Position * TriangularObstacle::getP3( void ){
	return this->p3;
}

int TriangularObstacle::include( Position * point ){
	int x = point->getPixelX();
	int y = point->getPixelY();

	int y1_2 = this->getPointEdge(point,p1,p2);
	int y1_3 = this->getPointEdge(point,p1,p3);
	int y2_3 = this->getPointEdge(point,p2,p3);

	bool onBorder = (y == y1_2 || y == y1_3 || y == y2_3);
	bool included = (
		((min(y1_2,y1_3) > 0) && ( y < max(y1_2,y1_3) && y > min(y1_2,y1_3)))
      ||((min(y1_2,y2_3) > 0) && ( y < max(y1_2,y2_3) && y > min(y1_2,y2_3)))
	  ||((min(y1_3,y2_3) > 0) && ( y < max(y1_3,y2_3) && y > min(y1_3,y2_3))));
	if(onBorder) {
		return 0;
	}
	if(included) {
		return 1;
	}
	return -1;
}

void TriangularObstacle::evaluateCollision( Ball * ball ){

	Collision * collision = NULL;

	//* Vertices *************************************************
	collision = evaluateCollisionVertex(ball, p1);
	ball->evaluateNextCollision(collision);
	collision = evaluateCollisionVertex(ball, p2);
	ball->evaluateNextCollision(collision);
	collision = evaluateCollisionVertex(ball, p3);
	ball->evaluateNextCollision(collision);

	//************************************************************
	try {
		collision = evaluateCollisionEdge(ball,p1,p2);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
	//************************************************************
	try {
		collision = evaluateCollisionEdge(ball,p1,p3);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
	//************************************************************
	try {
		collision = evaluateCollisionEdge(ball,p2,p3);
		ball->evaluateNextCollision(collision);
	}catch (NoIntersectException e){
	}
}


Collision * TriangularObstacle::evaluateCollisionEdge( Ball * ball, Position * point1, Position * point2){
	try {
		float PI = 3.14159;
		float dx = point2->getPixelX()-point1->getPixelX();
		float dy = point2->getPixelY()-point1->getPixelY();
		float m = ball->getM();
		float r = ball->getRadius();
		float b = ball->getPosition()->getPixelY();
		float xb = ball->getPosition()->getPixelX();
		float m2 = dy/dx;
		Position * leftPoint = point1->getPixelX() > point2->getPixelX()? point2 : point1;
		float b2 = leftPoint->getPixelY();
		float xb2 = leftPoint->getPixelX();
		Position * intersection = GeometryHelper::getInstance()->getIntersection(xb,m,b,xb2,m2,b2);
		int signo = GeometryHelper::getInstance()->sign(intersection->getPixelX()-xb);
		float edgeAngle = GeometryHelper::getInstance()->getAngle(m2, signo);
		float ballAngle = ball->getAngle(signo);
		float deltaX,deltaY;

		int minXEdge = min(point1->getPixelX(),point2->getPixelX());
		int maxXEdge = max(point1->getPixelX(),point2->getPixelX());
		int minYEdge = min(point1->getPixelY(),point2->getPixelY());
		int maxYEdge = max(point1->getPixelY(),point2->getPixelY());

		bool existIntersection = false;
		if(point1->getPixelX() != point2->getPixelX()) {
			if(ball->hasImpactSense(intersection)) {
				existIntersection = true;
				float normalAngle = GeometryHelper::getInstance()->correctAngle(edgeAngle - PI / 2,signo);
				float deltaAngle = normalAngle - ballAngle;
				float reflectAngle = normalAngle + deltaAngle;
				deltaX = cos(reflectAngle);
				deltaY = sin(reflectAngle);
			}
		} else {
			if( intersection->getPixelY() < maxYEdge + r && intersection->getPixelY() > minYEdge - r ) {
				if(ball->hasImpactSense(intersection)) {
					existIntersection = true;
					deltaX = cos(-ballAngle);
					deltaY = sin(-ballAngle);
				}
			}
		}
		// Si hay interseccion entre rectas construye la colision.
		Collision *	collision = NULL;
		float distanceUntilImpact = ball->calculateDistance(intersection);
		delete intersection;
		float distanceAdjust = abs(r/sin(ballAngle-edgeAngle));
		float adjustedDistance = distanceUntilImpact - distanceAdjust;
		// Centro de la bola al instante de la colision.
		Position * collisionPoint = ball->calculatePosition(adjustedDistance);
		// Punto de la recta que corresponde a la arista que esta mas cercano al determinado como colision.
		Position * impactPointOnEdge = GeometryHelper::getInstance()->getNearerPoint(collisionPoint,xb2,m2,b2); 
		// Existe interseccion si hay interseccion y el punto de impacto esta en la arista.
		bool existCollision = (existIntersection && minXEdge <= impactPointOnEdge->getPixelX() && impactPointOnEdge->getPixelX() <= maxXEdge && ball->hasImpactSense(collisionPoint));
		if(existCollision) {
			collision = new Collision();
			collision->setDistance(adjustedDistance);
			collision->setPoint(collisionPoint);
			collision->setDeltaX(deltaX);
			collision->setDeltaY(deltaY);
			collision->setElement(static_cast<Element*>(this));
		}
		return collision;
	}catch (ParallelStraightsException e) {
		throw NoIntersectException("La rectas son paralelas.");
	}
	throw NoIntersectException("La recta no intersecta la arista.");
}

Collision * TriangularObstacle::evaluateCollisionVertex( Ball * ball, Position * vertex ){

	Position * nearerPoint = GeometryHelper::getInstance()->getNearerPoint(vertex,ball->getPosition()->getPixelX(),ball->getM(),ball->getPosition()->getPixelY());
	float distanceToTrayectory = GeometryHelper::getInstance()->calculateDistance(vertex, nearerPoint);
	if(distanceToTrayectory < ball->getRadius()) {
		float distanceAdjust = ((float)pow((float)ball->getRadius(),2)-(float)pow((float)distanceToTrayectory,2));
		distanceAdjust = pow(distanceAdjust,0.5f);
		float distance = ball->calculateDistance(nearerPoint);
		float adjustedDistance = distance-distanceAdjust;
		Position * collisionPoint = ball->calculatePosition(adjustedDistance);

		if(ball->hasImpactSense(vertex)) {
			Collision * collision = new Collision();
			Position * referencePoint = ball->calculatePosition(adjustedDistance+ball->getRadius());
			float outAngle = GeometryHelper::getInstance()->getAngle(vertex, referencePoint);
			collision->setDeltaX(cos(outAngle));
			collision->setDeltaY(sin(outAngle));
			collision->setPoint(collisionPoint);
			collision->setDistance(adjustedDistance);
			collision->setElement(static_cast<Element*>(this));
			return collision;
		}
	}
	return NULL;
}

int TriangularObstacle::getPointEdge( Position * p, Position * point1, Position * point2){	
	float dx = point2->getPixelX()-point1->getPixelX();
	float dy = point2->getPixelY()-point1->getPixelY();
	float m2 = dy/dx;
	Position * leftPoint = point1->getPixelX() > point2->getPixelX()? point2 : point1;
	Position * rightPoint = point1->getPixelX() > point2->getPixelX()? point1 : point2;
	float b2 = leftPoint->getPixelY();
	float xb2 = leftPoint->getPixelX();
	int x = p->getPixelX();
	int y = p->getPixelY();
	float opuesto;
	float adyacente;
	int yEdge = -1;

	if(leftPoint->getPixelX() != rightPoint->getPixelX()) {
		if( x <= rightPoint->getPixelX() && x >= leftPoint->getPixelX() ) {
			yEdge = GeometryHelper::getInstance()->fx(x-leftPoint->getPixelX(),m2,b2); // Ecuacion de la recta edge 1.
		}
	}else {
		if(x == leftPoint->getPixelX() && y > min(leftPoint->getPixelY(),rightPoint->getPixelY()) && y < max(leftPoint->getPixelY(),rightPoint->getPixelY())) {
			yEdge = y; // Si la recta es vertical, en caso q la x coincida con el punto, el tambien lo hace.
		}
	}
	return yEdge;
}

//**************************************************************************************************


float TriangularObstacle::getMinXPos(){
	float minimo = min(this->getP1()->getAxisX(),this->getP2()->getAxisX());
	minimo =  min(minimo,this->getP3()->getAxisX());
	return minimo;
}

float TriangularObstacle::getMinYPos(){	
	float minimo = min(this->getP1()->getAxisY(),this->getP2()->getAxisY());
	minimo =  min(minimo,this->getP3()->getAxisY());
	return minimo;
}

float TriangularObstacle::getMaxXPos(){	
	float maximo = max(this->getP1()->getAxisX(),this->getP2()->getAxisX());
	maximo =  max(maximo,this->getP3()->getAxisX());
	return maximo;
}

float TriangularObstacle::getMaxYPos(){
	float maximo = max(this->getP1()->getAxisY(),this->getP2()->getAxisY());
	maximo =  max(maximo,this->getP3()->getAxisY());
	return maximo;
}

void TriangularObstacle::setPosition(Position * position){
	this->position = position;

	this->p1 = SketcherHelper::getInstance()->getPositionInPixels(p1);
	this->p2 = SketcherHelper::getInstance()->getPositionInPixels(p2);
	this->p3 = SketcherHelper::getInstance()->getPositionInPixels(p3);

	Position * leftPoint;
	Position * rightPoint;
	float opuesto;
	float adyacente;
	int yEdge = -1;
	// Arista 1-2.
	leftPoint = p1->getPixelX() > p2->getPixelX()? p2 : p1;
	rightPoint = p1->getPixelX() > p2->getPixelX()? p1 : p2;
	opuesto = rightPoint->getPixelY()-leftPoint->getPixelY();
	adyacente = rightPoint->getPixelX()-leftPoint->getPixelX();
	this->m1_2 = opuesto/adyacente;
	this->ord1_2 = leftPoint->getPixelY();
	// Arista 1-3.
	leftPoint = p1->getPixelX() > p3->getPixelX()? p3 : p1;
	rightPoint = p1->getPixelX() > p3->getPixelX()? p1 : p3;
	opuesto = rightPoint->getPixelY()-leftPoint->getPixelY();
	adyacente = rightPoint->getPixelX()-leftPoint->getPixelX();
	this->m1_3 = opuesto/adyacente;
	this->ord1_3 = leftPoint->getPixelY();
	// Arista 2-3.
	leftPoint = p2->getPixelX() > p3->getPixelX()? p3 : p2;
	rightPoint = p2->getPixelX() > p3->getPixelX()? p2 : p3;
	opuesto = rightPoint->getPixelY()-leftPoint->getPixelY();
	adyacente = rightPoint->getPixelX()-leftPoint->getPixelX();
	this->m2_3 = opuesto/adyacente;
	this->ord2_3 = leftPoint->getPixelY();

}

Position *TriangularObstacle::getPosition(){
	return this->position;
}

Color * TriangularObstacle::getElementColor(){
	return this->elementColor;
}

void TriangularObstacle::setElementColor(Color * color){
	this->elementColor = color;
}

Texture *TriangularObstacle::getTexture(){
	return this->texture;
}

void TriangularObstacle::setTexture(Texture *texture){
	this->texture = texture;
}

Color * TriangularObstacle::getLineColor()
{
	return this->lineColor;
}

void TriangularObstacle::setLineColor( Color * color )
{
	this->lineColor = color;
}

void TriangularObstacle::draw()
{
	Sketcher* sketcher = new TriangularSketcher(this);
	sketcher->draw();
	delete(sketcher);
}


